/* CPYHDR { */
/*
 * This file is part of the 'iit-rbd' library.
 * Copyright © 2015 2016, Marco Frigerio (marco.frigerio@iit.it)
 *
 * See the LICENSE file for more information.
 */
/* } CPYHDR */
#ifndef IIT_RBD_H_
#define IIT_RBD_H_

#include <Eigen/Dense>
#include <Eigen/StdVector>
#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET 1
#include <Eigen/Sparse>


namespace iit {
/**
 * This namespace contains some basic types and functions related to spatial
 * vectors and Rigid Body Dynamics (RBD).
 *
 * This code has been developed to provide basic types and general utilities to
 * be used in turn by the C++ classes generated by the Robotics Code Generator
 * (RobCoGen). That is, the code is just meant to be a common, generic facility
 * for all the robot-specific code that RobCoGen can generate.
 *
 * In other words, the primary target of the \c iit_rbd library is not to
 * provide a comprehensive object-oriented implementation of concepts of
 * rigid-body dynamics and spatial vectors, for end-user applications. Some
 * of such concepts do appear in this software, although the implementation is
 * relatively simple.
 * However, the library may very well be used in custom code that needs e.g.
 * a type for spatial vectors or for the spatial 6x6 inertia tensor.
 */
namespace rbd {

/**
 * \defgroup rbd-core RBD Core
 * Main, basic data types and functions
 * @{
 */

/**
 * \name General matrix types
 */
///@{
// Use alias templates to limit the references to 'Eigen' to this header

template<typename Derived>
using MatrixBase = Eigen::MatrixBase< Derived >;

template<typename Scalar, int R, int C>
using PlainMatrix = Eigen::Matrix< Scalar, R, C >;

template<typename XprType, int R, int C>
using MatrixBlock = Eigen::Block< XprType, R, C >;

template<typename Scalar>
using SparseMatrix = Eigen::SparseMatrix<Scalar>;

template<typename Scalar>
using SparseVector = Eigen::SparseVector<Scalar>;
///@}

/**
 * A container of basic type/function definitions, templated on the scalar type.
 *
 * \tparam SCALAR the numerical type for scalar values
 */
template<typename SCALAR>
struct Core
{
    typedef SCALAR Scalar;

    /** \name Basic matrix types */
    ///@{
    typedef PlainMatrix<Scalar,3,3> Matrix33;
    typedef PlainMatrix<Scalar,6,6> Matrix66;
    typedef PlainMatrix<Scalar,3,1> Vector3;
    typedef PlainMatrix<Scalar,6,1> Vector6;
    ///@}

    /**
     * \name 6D vectors "à la Featherstone"
     * Types of vectors used in dynamics computations.
     */
    ///@{
    typedef Vector6  Vector6D;       // here the 'D' stands for Dimension, not double !
    typedef Vector6D Column6D;
    typedef Vector6D VelocityVector;
    typedef Vector6D ForceVector;

    typedef MatrixBlock<Vector6D,3,1>       Part3D;     ///< a 3D subvector of a 6D vector
    typedef MatrixBlock<const Vector6D,3,1> Part3DConst;///< a const 3D subvector of a 6D vector
    ///@}


    /**
     *  \name 6D vectors accessors
     *  These functions allow to access the linear and the angular
     *  coordinates of motion/force vectors.
     */
    ///@{
    /**
     * The 3-coordinate vector with the angular components (angular
     * velocity or torque) of the given 6D vector.
     */
    static inline Part3D angularPart(Vector6D& f) {
        return f.template topRows<3>();
    }
    /**
     * The 3-coordinate vector with the linear components (linear
     * velocity or force) of the given 6D vector.
     */
    static inline Part3D linearPart(Vector6D& f) {
        return f.template bottomRows<3>();
    }
    static inline Part3DConst angularPart(const Vector6D& f) {
        return f.template topRows<3>();
    }
    static inline Part3DConst linearPart(const Vector6D& f) {
        return f.template bottomRows<3>();
    }
};


/**
 * \name Types with 'double' as the scalar type.
 * Explicit instantiation of all the various matrix types using the standard
 * \c double as the scalar type. See Core
 */
///@{
typedef Core<double> Cored;

using Matrix33d = Cored::Matrix33;
using Vector3d  = Cored::Vector3;
using Matrix66d = Cored::Matrix66;
using Vector6d  = Cored::Vector6;

using Vector6D       = Cored::Vector6D;
using VelocityVector = Cored::VelocityVector;
using ForceVector    = Cored::ForceVector;

using Part3D      = Cored::Part3D;
using Part3DConst = Cored::Part3DConst;

using Column6d = Cored::Column6D;  // an alias

using SparseMatrixd = SparseMatrix<double>;
using SparseColumnd = SparseVector<double>;


inline Part3D angularPart(Vector6D& f) {
    return Cored::angularPart(f);
}
inline Part3D linearPart(Vector6D& f) {
    return Cored::linearPart(f);
}
inline Part3DConst angularPart(const Vector6D& f) {
    return Cored::angularPart(f);
}
inline Part3DConst linearPart(const Vector6D& f) {
    return Cored::linearPart(f);
}
///@}

/**
 * \name Vector coordinates
 * Constants to index either 6D or 3D coordinate vectors.
 */
///@{
enum Coords3D { X=0, Y, Z};
/// To be used with 6D vectors. 'A' stands for angular, 'L' for linear.
enum Coords6D { AX=0, AY, AZ, LX, LY, LZ };
///@}

///@} // end of implicit in-group elements [Doxygen]

/**
 * The Earth gravitational acceleration constant.
 * This value should always be positive, so that an application
 * chooses the sign according to its conventions.
 */
static const double g = 9.81;

}
}


#endif /* IIT_RBD_H_ */
